//system files
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
//inner files
#include "gps_linearize.h"
#define REQ 6378135.0
#define RPO 6356750.0

/* Useful links:
   http://www.movable-type.co.uk/scripts/LatLongVincenty.html
   http://en.wikipedia.org/wiki/Earth_radius
*/

double GPSCoordinateTool::sq(double v)
{
  return v*v;
}

void GPSCoordinateTool::gps_linearize_init(const double ll_deg[2])
{
    origin_.lat0_deg = ll_deg[0];
    origin_.lon0_deg = ll_deg[1];

    double a = 6378137;  // R_equator
    double b = 6356752;  // R_polar

    double lat_rad = to_radians(ll_deg[0]);

    // this is the best radius approximation, agnostic of direction
    // we don't use this anymore.
    //    origin_.radius = a*a*b / (sq(a*cos(lat_rad)) + sq(b*sin(lat_rad)));

    // best radius approximation in ns and ew direction.
    origin_.radius_ns = sq(a*b) / pow((sq(a*cos(lat_rad))) + sq(b*sin(lat_rad)), 1.5);
    origin_.radius_ew = a*a / sqrt(sq(a*cos(lat_rad)) + sq(b*sin(lat_rad)));
}

//輸入 ll_deg[0]=lat緯度  ll_deg[1]=lon經度
//輸出xy[0]=x xy[1]=y
int GPSCoordinateTool::gps_linearize_to_xy(const double ll_deg[2], double xy[2])
{
    double dlat = to_radians(ll_deg[0] - origin_.lat0_deg);
    double dlon = to_radians(ll_deg[1] - origin_.lon0_deg);

    xy[0] = sin(dlon) * origin_.radius_ew * cos(to_radians(origin_.lat0_deg));//x軸數值 經度差
    xy[1] = sin(dlat) * origin_.radius_ns;//y軸數值 緯度差
    
    return 0;
}

int GPSCoordinateTool::gps_linearize_to_lat_lon(const double xy[2], double ll_deg[2])
{
    double dlat = asin(xy[1] / origin_.radius_ns);
    ll_deg[0] = to_degrees(dlat) + origin_.lat0_deg;

    double dlon = asin(xy[0] / origin_.radius_ew / cos(to_radians(origin_.lat0_deg)));
    ll_deg[1] = to_degrees(dlon) + origin_.lon0_deg;

    return 0;
}

int GPSCoordinateTool::gps_linearize_to_lat_lon(const double x,const double y, double *lat,double *lon)
{
    double dlat = asin(y / origin_.radius_ns);
    *lat = to_degrees(dlat) + origin_.lat0_deg;

    double dlon = asin(x / origin_.radius_ew / cos(to_radians(origin_.lat0_deg)));
    *lon = to_degrees(dlon) + origin_.lon0_deg;

    return 0;
}

void GPSCoordinateTool::vRotationtoxy(double *dxy, double *newy, double rota)
{
    newy[0] = dxy[0]*cos(rota) + dxy[1]*sin(rota);
    newy[1] = dxy[1]*cos(rota) - dxy[0]*sin(rota);
}

void GPSCoordinateTool::vRotationTransform(double *dxy, double *newy, double *transxy, double rota)
{
    double rotTransxy_l[2];
    double dxy_l[2];
    vRotationtoxy(dxy,dxy_l,rota);
    vRotationtoxy(transxy,rotTransxy_l,rota);
    newy[0] = dxy_l[0] - rotTransxy_l[0];
    newy[1] = dxy_l[1] - rotTransxy_l[1];

} 


//[0] x,[1] y, [2] theta 车头 朝向
void GPSCoordinateTool::Gloable2Relative(double *gloable, double *local, double *vehicle)
{
    local[0] = (gloable[0] - vehicle[0])*cos(vehicle[2]-M_PI/2)+(gloable[1] - vehicle[1])*sin(vehicle[2]-M_PI/2);
    local[1] = -(gloable[0] - vehicle[0])*sin(vehicle[2]-M_PI/2)+(gloable[1] - vehicle[1])*cos(vehicle[2]-M_PI/2);
    local[2] = gloable[2] - vehicle[2] + M_PI/2;
}

//[0] x,[1] y, [2] theta
void GPSCoordinateTool::Relative2Gloable(double *local, double *gloable, double *vehicle)
{
    gloable[0] = local[0]*cos(vehicle[2]-M_PI/2) - local[1]*sin(vehicle[2]-M_PI/2) + vehicle[0];
    gloable[1] = local[0]*sin(vehicle[2]-M_PI/2) + local[1]*cos(vehicle[2]-M_PI/2) + vehicle[1];
    gloable[2] = vehicle[2]- M_PI / 2 + local[2];
}


double GPSCoordinateTool::DistancePoint2Point(double *point1,double *point2)
{
    return std::hypot(point1[0]-point2[0],point1[1]-point2[1]);
}
